#include <stdio.h>
#include <math.h>

#include "IncrementalNewtonSolver.h"
#include "LinearCGState.h"
#include "DeformationState.h"
#include "BigVector.h"
#include "Setting.h"

static Real tolerance;
int MAX_ITER;

IncrementalNewtonSolver::IncrementalNewtonSolver(DeformationState *ps)
{
	if (!MAX_ITER)
	{
		MAX_ITER = (int) GetNumberSetting ("max-cg-iterations");
	}

	pstate_ = ps;
	cg_state_ = new LinearCGState;
	cg_state_-> residual_tolerance_ = GetNumberSetting("incremental-tolerance");
	tolerance = GetNumberSetting ("outer-loop-tolerance");

	ps->incremental_b_ = true;
}

bool
IncrementalNewtonSolver::GoodSolution (DeformationState const* p)const
{
	IncrementalNewtonSolver *me = (IncrementalNewtonSolver*)this;
	Real reslen = p->ComputeResidualForce (me->residual_.UnsafeAccessArray (),
		p->displacement_.AccessArray ());

	bool b =  (reslen <= Sqr(tolerance) * p->external_force_len_sq_);


	return b;
}

static void ComputeIncrementalElasticForce (void *info, Real * force, Real const *dis)
{
	IncrementalNewtonSolver * me = (IncrementalNewtonSolver*)info;

	me->pstate_->ApplyForceDerivativeAndResidual (force,
		0,
		me->pstate_->displacement_.AccessArray(),
		dis);
	BigVectorNegate (force,force, me->Dimension());
}


LinkArray< Array<Real> > IncrementalNewtonSolver::VectorVariables ()
{
	Array<Real> *vecs[]
	= { &direction_, &residual_ ,0};

	LinkArray< Array<Real> > vvs;
	for (int i= 0; vecs[i]; i++)
		vvs.Push (vecs[i]);

	vvs.Concat (cg_state_->VectorVariables());
	return vvs;
}



void IncrementalNewtonSolver::IterationStep (DeformationState *p)
{
	int n = pstate_->Dimension();
	Real * cg_res = cg_state_->residual_.UnsafeAccessArray();
	Real * dir = direction_.UnsafeAccessArray ();

	Real reslen = p->ComputeResidualForce (residual_.UnsafeAccessArray (),
		p->displacement_.AccessArray ());

	pstate_->UpdateIncrementalStep ();

	printf (" reslen : %lf ", reslen);  
	cg_state_->CGReinit();
	BigVectorNullify (dir, n);

	BigVectorNegate (cg_state_->residual_.UnsafeAccessArray (),
		residual_.AccessArray(),n);

	cg_state_->residual_len_ = reslen;

	while ((!MAX_ITER || cg_state_->iter_count_ < MAX_ITER)
		&& !cg_state_->CGGoodSolution (reslen))
	{
		cg_state_->CGIterationStep (dir,
			pstate_->fixed_arr_.AccessArray (),
			(void*) this,
			&ComputeIncrementalElasticForce);

		cg_state_->prev_residual_len_ = cg_state_->residual_len_;
		cg_state_->residual_len_ = BigVectorIP (cg_res,cg_res,n);
		cg_state_->iter_count_ ++;
	}

	printf (" cg iters : %d\n ", cg_state_->iter_count_);
	BigVectorSubtract  (p->displacement_.UnsafeAccessArray (),
		p->displacement_.AccessArray (),
		dir,
		n);
}

void
IncrementalNewtonSolver::UpdateResidual (DeformationState const*)
{

}

void IncrementalNewtonSolver::Reinit(DeformationState const*)
{

}

IncrementalNewtonSolver::~IncrementalNewtonSolver()
{

}
